/*
 * @Description:
 * @Version:
 * @Author: Troubadour
 * @Date: 2022-08-31 21:57:58
 * @LastEditors: Troubadour
 * @LastEditTime: 2022-10-07 21:00:06
 */

#include "RobotServoFw.h"
#include "RobotArray.h"


/**
 * @function: RobotServoFw
 * @brief 默认构造函数
 * @param {*}
 * @return {*}
 */
RobotServoFw::RobotServoFw()
{
    SerialX = &Serial;
}


/**
 * @function: RobotServoFw
 * @brief 带指定串口ID的构造函数
 * @param {HardwareSerial} &Handle
 * @return {*}
 */
RobotServoFw::RobotServoFw(HardwareSerial &Handle)
{
    SerialX = &Handle;
}


/**
 * @function: ~RobotServoFw
 * @brief 析构函数
 * @param {*}
 * @return {*}
 */
RobotServoFw::~RobotServoFw()
{
}


/**
 * @function: LobotCheckSum
 * @brief 计算舵机协议校验和
 * @param {uint8_t} *buf：协议数据
 * @return {uint8_t} 返回校验结果
 */
uint8_t RobotServoFw::LobotCheckSum(uint8_t (&buf)[CMD_Size])
{
    uint8_t i;
    uint16_t temp = 0;
    for (i = 2; i < buf[3] + 2; i++) {
    temp += buf[i];
    }
    temp = ~temp;
    i = (uint8_t)temp;
    return i;
}


/**
 * @function: LobotSerialServoMove
 * @brief 控制单个舵机转动
 * @param {uint8_t} id：舵机ID
 * @param {int16_t} position：舵机位置，范围（0~1000）
 * @param {uint16_t} time
 * @return {*}
 */
void RobotServoFw::LobotSerialServoMove(uint8_t id, int16_t position, uint16_t time)
{
    uint8_t buf[20];
    if(position < 0)
        position = 0;
    if(position > 1000)
        position = 1000;
    buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER;
    buf[2] = id;
    buf[3] = 7;
    buf[4] = LOBOT_SERVO_MOVE_TIME_WRITE;
    buf[5] = GET_LOW_BYTE(position);
    buf[6] = GET_HIGH_BYTE(position);
    buf[7] = GET_LOW_BYTE(time);
    buf[8] = GET_HIGH_BYTE(time);
    buf[9] = LobotCheckSum(buf);

    SerialX->write(buf, 10);
}


/**
 * @function: Robot_ActionArray
 * @brief 执行机器人动作组
 * @param {uint16_t} ActionArray：一个二维数组，每行包含6个舵机参数+运行时间
 * @param {uint8_t} size：二维数据大小（多少行）
 * @return {*}
 */
void RobotServoFw::Robot_ActionArray(const uint16_t ActionArray[][Array_size], uint8_t size)
{
    uint8_t i = 0, j = 0;
    for (i = 0; i < size; i++)
    {
        for (j = 0; j < Array_size; j++)
        {
            LobotSerialServoMove(j+1, ActionArray[i][j], ActionArray[i][Array_size-1]);
        }
        delay(ActionArray[i][Array_size-1]);
    }
}


/**
 * @function: Robot_Running
 * @brief 执行前进动作
 * @param {*}
 * @return {*}
 */
void RobotServoFw::Robot_Running(void)
{
    Robot_ActionArray(Run_Array, sizeof(Run_Array)/sizeof(Run_Array[0]));
}


/**
 * @function: Robot_Back
 * @brief 执行后退动作
 * @param {*}
 * @return {*}
 */
void RobotServoFw::Robot_Back(void)
{
    Robot_ActionArray(Back_Array, sizeof(Back_Array)/sizeof(Back_Array[0]));
}


/**
 * @function: Robot_LeftHanded
 * @brief 执行左转向动作
 * @param {*}
 * @return {*}
 */
void RobotServoFw::Robot_LeftHanded(void)
{
    Robot_ActionArray(LeftHanded_Array, sizeof(LeftHanded_Array)/sizeof(LeftHanded_Array[0]));
}


/**
 * @function: Robot_Running
 * @brief 执行右转向动作
 * @param {*}
 * @return {*}
 */
void RobotServoFw::Robot_RightHanded(void)
{
    Robot_ActionArray(RightHanded_Array, sizeof(RightHanded_Array)/sizeof(RightHanded_Array[0]));
}


/**
 * @function: Robot_Stand
 * @brief 执行立正动作
 * @param {*}
 * @return {*}
 */
void RobotServoFw::Robot_Stand(void)
{
    Robot_ActionArray(Stand_Array, sizeof(Stand_Array)/sizeof(Stand_Array[0]));
}


/**
 * @function: Robot_ForwardRoll
 * @brief 前翻滚动作组
 * @param {*}
 * @return {*}
 */
void RobotServoFw::Robot_ForwardRoll(void)
{
    Robot_ActionArray(ForwardRoll_Array, sizeof(ForwardRoll_Array)/sizeof(ForwardRoll_Array[0]));
}


/**
 * @function: Robot_BackwardRoll
 * @brief 后翻滚动作组
 * @param {*}
 * @return {*}
 */
void RobotServoFw::Robot_BackwardRoll(void)
{
    Robot_ActionArray(BackwardRoll_Array, sizeof(BackwardRoll_Array)/sizeof(BackwardRoll_Array[0]));
}


/**
 * @function: Robot_BackwardRoll
 * @brief 下蹲动作组
 * @param {*}
 * @return {*}
 */
void RobotServoFw::Robot_Crouch(void)
{
    Robot_ActionArray(Crouch_Array, sizeof(Crouch_Array)/sizeof(Crouch_Array[0]));
}







